#ifndef MAP_LOCAL_H
#define MAP_LOCAL_H

#include "map_global.h"
#include "robot_obts.h"
#include <opencv2/opencv.hpp>
#include "common_functions.h"
#include <iterator>  // std::begin, std::end





class MapLocal 
{
public:
    MapLocal(int width, int height, float grid_size);
    ~MapLocal();

    void updateFromGlobalMap(const MapGlobal& global_map, int robot_x, int robot_y);
    void update(const std::vector<unsigned char>& map_ptr,  std::vector<cv::Point3f> pts, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy, int i);
    std::vector<unsigned char> getUpdatedLocalMapData() const ;

    // std::vector<unsigned char> priorMap(const unsigned char* map_ptr,) const;

private:
    void transformCoords(const cv::Point3f& local_points, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy);
    void transformPoints(std::vector<cv::Point3f> pts, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy);

    void handleCurFrameData(std::vector<cv::Point3f> pts, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy, int sensor_index);
    void handlePreFrameData(const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy, int sensor_index);
    void calcCamLocalPose(const cv::Point3f& fusepose_rpy);
    void printMap() const;

    // global map
    void getPriorMap(const std::vector<unsigned char>& map_ptr, int width, int height, const cv::Point3f& fusepose_xyz, cv::Point3f& fusepose_rpy);


    // draw visual
    void drawCarPose(cv::Mat &img, std::vector<cv::Point> carPostion);
    // int float2int(float prob);
    // float int2float(int vlaue);
private:
    int width_, height_, size_;
    float grid_size_;
    unsigned char* map_cur_;
    unsigned char* map_pre_;
    FrameData frame_data_pre_;
    FrameData frame_data_cur_;
    FrameData frame_data_tmp_;

    const std::vector<unsigned char> map_ref_;
    // std::vector<cv::Point2d> 


private:
    float MAP_SIZE = 2.0; // 2M
    float GRID_SIZE = 0.05; // every grid mean 0.05m
    int LOCAL_CENTER = 50; 
};

#endif // MAP_LOCAL_H
